#include <iostream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/filters/passthrough.h>//直通滤波
#include <pcl/filters/statistical_outlier_removal.h>//统计滤波
#include <pcl/filters/random_sample.h>//点云降采样
#include <pcl/filters/voxel_grid.h>//随机采样

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

void visualizeCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_cloud);
int main() {
    std::string cloud_path = "./pointsData/Horse.pcd";

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(cloud_path.c_str(), *cloud);
    std::cout << "加载点云" << cloud->points.size() << "个" << std::endl;

    // // 创建滤波器对象
    // pcl::PassThrough<pcl::PointXYZ> pass;
    // pass.setInputCloud(cloud);
    // pass.setFilterFieldName("z"); // 滤波字段名被设置为z轴方向
    // pass.setFilterLimits(0.0, 1.0); // 设置在过滤方向上的过滤范围
    // pass.setNegative(true); // 设置保留范围内的点还是过滤掉范围内的点，标志为false时保留范围内的点
    // pass.filter(*cloud_filtered);

    //创建统计滤波器对象
    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ>  sor_OutRemove;  
    // //输入点云 
    // sor_OutRemove.setInputCloud (cloud); 
    // //设置在进行统计时考虑查询点邻近点数
    // sor_OutRemove.setMeanK (30);  
    // //设置判断是否为离群点的阈值
    // sor_OutRemove.setStddevMulThresh (1.0); 
    // // 执行滤波返回滤波后的点云 
    // sor_OutRemove.filter (*cloud_filtered);

    //创建点云降采样滤波器对象	
    // pcl::RandomSample< pcl::PointXYZ > rs;
    // //设置输入点云
    // rs.setInputCloud(cloud);	
    // //设置下采样要保留点云的点数
    // rs.setSample(20000);
    // //设置随机函数种子点
    // //rs.setSeed(1);
    // //执行下采样滤波	
    // rs.filter(*cloud_filtered);

    //创建体素栅格采样滤波器对象
    pcl::VoxelGrid<pcl::PointXYZ> vox;
    //设置输入点云
    vox.setInputCloud(cloud);
    //设置最小体素边长
    vox.setLeafSize(0.01f, 0.01f, 0.01f);
    //执行滤波采样
    vox.filter(*cloud_filtered);



    std::cout << "滤波后的点云" << cloud_filtered->points.size() << std::endl;
    visualizeCloud(cloud, cloud_filtered); 


    return 0;
}

void visualizeCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_cloud) {
	//---------显示点云-----------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("显示点云"));

	int v1(0), v2(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("point clouds", 10, 10, "v1_text", v1);
	viewer->createViewPort(0.5, 0.0, 1, 1.0, v2);
	viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
	viewer->addText("filtered point clouds", 10, 10, "v2_text", v2);
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "z"); // 按照z字段进行渲染,将z改为x或y即为按照x或y字段渲染
	viewer->addPointCloud<pcl::PointXYZ>(cloud, fildColor, "sample cloud", v1);

        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor_filtered(cloud, "z"); // 按照z字段进行渲染,将z改为x或y即为按照x或y字段渲染
	viewer->addPointCloud<pcl::PointXYZ>(filter_cloud, fildColor_filtered,"cloud_filtered", v2);
	//viewer->addCoordinateSystem(1.0);
	//viewer->initCameraParameters();
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}


